#!/usr/bin/python3
import serial
import time
import rospy

from drone_pkg.msg import Temperature


rospy.init_node('temperature', anonymous=True)
msg = Temperature()
pub = rospy.Publisher('/temperature', Temperature, queue_size=10)


def parse_serial_packet(packet_hex):

    """
    解析串口数据包
    格式: 5B 06 00 1F 8F 1B 35 1E 06 1E 2C 11 05 00 01 82
    - 5B 06: 帧头
    - 00: SID设备码
    - 1F 8F: 最高温度数据
    - 1B 35: 最低温度
    - 1E 06: 中心温度
    - 1E 2C: 平均温度
    - 11 05: 最高温度坐标
    - 00: 报警状态
    - 01 82: CRC校验码(byte3~13的累加和)
    """
    # 将16进制字符串转换为字节列表
    bytes_list = [int(x, 16) for x in packet_hex.split()]
    
    # 验证帧头
    if bytes_list[0] != 0x5B or bytes_list[1] != 0x06:
        raise ValueError("无效的帧头")
    
    # 计算CRC校验
    crc_calculated = sum(bytes_list[2:14]) & 0xFFFF  # 取低16位
    crc_received = (bytes_list[14] << 8) | bytes_list[15]
    
    if crc_calculated != crc_received:
        raise ValueError(f"CRC校验失败: 计算值={hex(crc_calculated)}, 接收值={hex(crc_received)}")
    
    # 解析数据
    sid = bytes_list[2]
    
    # 温度数据转换 (假设为16位有符号整数, 单位0.01°C)
    max_temp = ((bytes_list[3] << 8) | bytes_list[4]) / 100 - 40
    min_temp = ((bytes_list[5] << 8) | bytes_list[6]) / 100 - 40
    center_temp = ((bytes_list[7] << 8) | bytes_list[8]) / 100 - 40 
    avg_temp = ((bytes_list[9] << 8) | bytes_list[10]) / 100 - 40
    
    # 最高温度坐标
    max_temp_x = bytes_list[11]
    max_temp_y = bytes_list[12]
    
    # 报警状态
    alarm_status = bytes_list[13]
    
    return {
        'frame_header': f"{bytes_list[0]:02X} {bytes_list[1]:02X}",
        'sid': sid,
        'max_temperature': max_temp,
        'min_temperature': min_temp,
        'center_temperature': center_temp,
        'average_temperature': avg_temp,
        'max_temp_coordinates': (max_temp_x, max_temp_y),
        'alarm_status': alarm_status,
        'crc_received': crc_received,
        'crc_calculated': crc_calculated,
        'crc_valid': crc_calculated == crc_received
    }

def read_serial_data(serial_port, baudrate=460800, timeout=1):
    """
    从串口读取数据并解析
    :param serial_port: 串口设备路径 (如 '/dev/ttyUSB0' 或 'COM3')
    :param baudrate: 波特率
    :param timeout: 超时时间(秒)
    """
    global pub, msg
    try:
        # 打开串口
        ser = serial.Serial(serial_port, baudrate, timeout=timeout)
        print(f"已连接串口 {serial_port}, 波特率 {baudrate}")
        

        # 发送初始化命令 5A 01 03
        init_command = bytes([0x5A, 0x01, 0x03])
        ser.write(init_command)
        print(f"已发送初始化命令: {' '.join(f'{b:02X}' for b in init_command)}")
        
        # 缓冲区用于存储接收到的数据
        buffer = bytearray()
        
        while (not rospy.is_shutdown()):
            # 读取串口数据
            data = ser.read(ser.in_waiting or 1)
            if data:
                buffer.extend(data)
                
                # 查找帧头 (0x5B 0x06)
                while len(buffer) >= 2:
                    if buffer[0] == 0x5B and buffer[1] == 0x06:
                        # 检查是否有完整的数据包 (16字节)
                        if len(buffer) >= 16:
                            packet_bytes = buffer[:16]
                            # 转换为16进制字符串格式
                            packet_hex = ' '.join(f"{b:02X}" for b in packet_bytes)
                            
                            try:
                                # 解析数据包
                                parsed_data = parse_serial_packet(packet_hex)
                                print("\n" + "="*50)
                                print(f"数据包解析结果 @ {time.strftime('%Y-%m-%d %H:%M:%S')}")
                                print("="*50)
                                print(f"帧头: {parsed_data['frame_header']}")
                                print(f"设备码(SID): {parsed_data['sid']}")
                                print(f"最高温度: {parsed_data['max_temperature']:.2f}°C")
                                print(f"最低温度: {parsed_data['min_temperature']:.2f}°C")
                                print(f"中心温度: {parsed_data['center_temperature']:.2f}°C")
                                print(f"平均温度: {parsed_data['average_temperature']:.2f}°C")
                                print(f"最高温度坐标(X,Y): {parsed_data['max_temp_coordinates']}")
                                print(f"报警状态: {'报警' if parsed_data['alarm_status'] else '正常'}")
                                print(f"接收的CRC: {hex(parsed_data['crc_received'])}")
                                print(f"计算的CRC: {hex(parsed_data['crc_calculated'])}")
                                print(f"CRC校验: {'通过' if parsed_data['crc_valid'] else '失败'}")
                                msg.max = parsed_data['max_temperature']
                                msg.min = parsed_data['min_temperature']
                                msg.mid = parsed_data['center_temperature']
                                msg.average = parsed_data['average_temperature']
                                (msg.x ,msg.y) = parsed_data['max_temp_coordinates']
                                pub.publish(msg)
                                # 移除已处理的数据包
                                buffer = buffer[16:]
                            except ValueError as e:
                                print(f"解析错误: {e}")
                                # 移除错误的帧头，继续查找下一个
                                buffer.pop(0)
                        else:
                            # 数据包不完整，等待更多数据
                            break
                    else:
                        # 不是帧头，移除第一个字节继续查找
                        buffer.pop(0)
            else:
                # 没有数据时稍微等待
                time.sleep(0.1)
                
    except serial.SerialException as e:
        print(f"串口错误: {e}")
    except KeyboardInterrupt:
        print("\n用户中断，停止读取")
    finally:
        if 'ser' in locals() and ser.is_open:
            ser.close()
            print("串口已关闭")

if __name__ == "__main__":
    serial_port = '/dev/ttyUSB0' 
    
    print("开始监听串口数据... (按Ctrl+C停止)")
    read_serial_data(serial_port)